#include "beeldverwerking.h"
#include "qtwindow.h"
#include "windows.h"

VideoCapture cap;
Mat imgTemplate;
Mat imgTemplate2;
Mat frame;
bool bWork = true;
bool bCamConnect = false;
bool bCamIsConnected = false;
bool bCreatePlayField = false;
int TriangleMatchOke = 0;
unsigned char thresHold = 100;
unsigned char camSelect = 0;
Point matchLocation;
Point matchRedLocation;
Point matchGreenLocation;


int detectTriangleRotation(const Mat* src);
int detectStartStop(const Mat* src, unsigned int pos[4]);

void BGR2RGB(Mat foo) { // Converts from BGR to RGB format or vice versa. Call this before sending away image data!
    for(int i = 0; i < foo.rows; i++)
    {
        for(int j = 0; j < foo.cols; j++)
        {
            Vec3b bgrPixel = foo.at<Vec3b>(i, j);
            uint8_t tmp = bgrPixel[0];
            bgrPixel[0] = bgrPixel[2];
            bgrPixel[2] = tmp;
            foo.at<Vec3b>(i, j) = bgrPixel;
        }
    }
    return;
}

int beeldverwerkingInit(){
    cout << "ImgPcs: init opencv" << endl;
    setUseOptimized(true);
    setNumThreads(10);
    cap.open(camSelect);
    if(!cap.isOpened()) {
        cout << "Failed to open camera." << std::endl;
        return 1;
    }
    //cvNamedWindow("LiveCamTest",CV_WINDOW_AUTOSIZE);

    cap >> frame;

    imgTemplate = imread("Template_1.bmp", CV_LOAD_IMAGE_GRAYSCALE);
    imgTemplate2 = imread("Template_2.bmp", CV_LOAD_IMAGE_GRAYSCALE);
    if(!imgTemplate.data || !imgTemplate2.data){
        cout << "ImgPcs: error loading template image" << endl;
        return 2;
    }
    return 0;
}
void beeldverwerkingDeinit(){
    cout << "ImgPcs: deinit opencv" << endl;
    //Destroy openCV items
	cap.release();
    cvDestroyAllWindows();
}
int beeldverwerkingMain(){
    cout << "ImgPcs: start" << endl;

    while(bWork){
        if(bCamIsConnected){
            if(bCamConnect){
                //Opencv loop
                cap >> frame;
				if(frame.data == NULL) {
					cap.release();
					bCamConnect = true;
					bCamIsConnected = false;
					cout << "Camera failed to fetch frame.\n";
				} else {

					//imshow("LiveCamTest",frame);

					//convert to RGB for preview
					Mat camCopy = frame.clone();
					BGR2RGB(camCopy);

					Scalar color;
					if(TriangleMatchOke == 0)
						color = Scalar(255,0,0);
					else if(TriangleMatchOke == 1)
						color = Scalar(255,255,0);
					else
						color = Scalar(0,255,0);

					//create playfield
					if(bCreatePlayField){
						// Mat frame

						// Convert img from RGB to HSV
						Mat frameHSV;
						cvtColor(frame,frameHSV,CV_RGB2HSV);

						// Extract V channel
						Mat img_gray;
						vector<cv::Mat> v_channel;
						frameHSV.copyTo(img_gray);
						split(img_gray,v_channel);

						img_gray = v_channel[2];

						// Treshold V channel on val=thresHold
						Mat img_display;
						img_gray.copyTo( img_display );
						threshold(img_gray, img_gray, thresHold, 255, 0);

                        // Dilate
						int dilation_size = 1;
						Mat elementDilation = getStructuringElement(MORPH_RECT,
															Size( 2*dilation_size + 1, 2*dilation_size+1 ),
                                                            Point( dilation_size, dilation_size ) );

						// Apply the dilation operation
						Mat img_dilated;
						dilate( img_gray, img_dilated, elementDilation );

						// Erode
						int erosion_size = 1;
						Mat elementErosion = getStructuringElement(MORPH_RECT,
															Size( 2*erosion_size + 1, 2*erosion_size+1 ),
															Point( erosion_size, erosion_size ) );

						// Apply the erosion operation
						Mat img_eroded;
						erode( img_dilated, img_eroded, elementErosion );
						img_gray = img_eroded;						// Detect start and end point
						// Extract start and end point out of image
						// Paste the start and end point onto the RGB speelveld

                        unsigned int Positions[4] = {0};
                        int ret = 0;
                        if((ret = detectStartStop(&frameHSV, Positions)) >= 0){
                            // Convert to RGB
                            Mat speelveld;
                            v_channel[0] = img_gray;
                            v_channel[1] = img_gray;
                            v_channel[2] = img_gray;
                            merge(v_channel,speelveld);

                            frame = speelveld;
                            BGR2RGB(frame);
                            writeSpeelveld(frame.data,frame.cols,frame.rows,Positions[0],Positions[1],Positions[2],Positions[3]);

                            rectangle( camCopy, matchRedLocation, Point( matchRedLocation.x + imgTemplate2.cols,
                                                                         matchRedLocation.y + imgTemplate2.rows), Scalar(255,0,0), 5, 10, 0 );
                            rectangle( camCopy, matchGreenLocation, Point( matchGreenLocation.x + imgTemplate2.cols,
                                                                         matchGreenLocation.y + imgTemplate2.rows), Scalar(0,255,0), 5, 10, 0 );
                            writeCamera(camCopy.data,camCopy.cols,camCopy.rows);

                            bCreatePlayField = false;
                        }
                        else{
                            cout << "ImgPcs: error from detectStartStop(); err no.:" << ret << endl;
                        }
					}
					else{ //do monitor orientation
						Mat* img_Hsv = new Mat(frame);
						cvtColor(frame, *img_Hsv, CV_BGR2HSV);

						int ret = 0;
						if((ret = detectTriangleRotation(img_Hsv)) >= 0){
						}
						else{
							//cout << "ImgPcs: error from detectTriagleRotation(); err no.:" << ret << endl;
						}

                        rectangle( camCopy, matchLocation, Point( matchLocation.x + imgTemplate.cols,
                               matchLocation.y + imgTemplate.rows ), color, 5, 10, 0 );
                        writeCamera(camCopy.data,camCopy.cols,camCopy.rows);

						delete img_Hsv;
					}
                    cvWaitKey(10);
				}
            }
            else{
                //Disconnect cam
                beeldverwerkingDeinit();
                bCamIsConnected = false;
            }
        }
        else{
            if(bCamConnect){
                //Connect cam
                if(beeldverwerkingInit() == 0)
                    bCamIsConnected = true;
                else
                    cout << "ImgPcs: cam init failed" << endl;
            }
            else{
                //Do nothing; Sleep
            }
        }
		Sleep(1);

        //Check for new assignments
        assignment* cmd;
        cmd = getAssignment();
		while (cmd != NULL){
            switch(cmd->type){
            case ASSIGNMENT_CREATE_SPEELVELD:
                cout << "ImgPcs: assignment; create speelveld" << endl;
                bCreatePlayField = true;
                delete cmd;
                break;

            case ASSIGNMENT_MONITOR_ORIENTATION:
                cout << "ImgPcs: assignment; monitor orientation" << endl;
                bCreatePlayField = false;
                delete cmd;
                break;

            case ASSIGNMENT_SHUTDOWN:
                cout << "ImgPcs: assignment; shutdown" << endl;
                bWork = false;
                delete cmd;
                break;

            case ASSIGNMENT_CAM_THRESHOLD:
                cout << "ImgPcs: assignment; threshold; " << (int)cmd->thresHold << endl;
                thresHold = cmd->thresHold;
                delete cmd;
                break;

            case ASSIGNMENT_CAM_CONNECT_DISCONNECT:
                bCamConnect = cmd->camConnect;
                camSelect = cmd->camSelect;
                cout << "ImgPcs: assignment; connect/disconnect; ";
                cout << "cam; " << (int)camSelect;
                cout << " connect; " << (int)bCamConnect << endl;
                delete cmd;
                break;

            default:
                cout << "ImgPcs: error unkown assignment; " << cmd->type << endl;
                delete cmd;
                break;
            }
			cmd = getAssignment();
		}
    }

    beeldverwerkingDeinit();
    cout << "ImgPcs: exit" << endl;
    return 0;
}

int detectTriangleRotation(const Mat* src){
    //matgray -> img_gray
    float angle = 0;
    //TriangleMatchOke = 0;
    /************************************************************
    * [Template Matching Method]
    * Detect the location of the triagle using a template
    ************************************************************/

    /// split HSV into three channels, [0]H,[1]S,[2]V
    Mat img_gray;
    vector<cv::Mat> v_channel;
    src->copyTo(img_gray);
    split(img_gray,v_channel);

    /// use only the Value channel
    img_gray = v_channel[2];

    /// threshold
    Mat img_display;
    img_gray.copyTo( img_display );
    threshold(img_gray, img_gray, thresHold, 255, 0);

    /// create the result matrix
    Mat img_result;
    int img_result_cols =  img_gray.cols - imgTemplate.cols + 1;
    int img_result_rows = img_gray.rows - imgTemplate.rows + 1;
    img_result.create( img_result_cols, img_result_rows, CV_32FC1 );

    /// do the Matching and Normalize
    int match_method = 0;
    matchTemplate( img_gray, imgTemplate, img_result, match_method );
    normalize( img_result, img_result, 0, 1, NORM_MINMAX, -1, Mat() );

    /// localizing the best match with minMaxLoc
    double minVal; double maxVal; Point minLoc; Point maxLoc;
    minMaxLoc( img_result, &minVal, &maxVal, &minLoc, &maxLoc, Mat() );

    /// for SQDIFF and SQDIFF_NORMED, the best matches are lower values. For all the other methods, the higher the better
    Point matchLoc;
    if( match_method  == CV_TM_SQDIFF || match_method == CV_TM_SQDIFF_NORMED )
    { matchLoc = minLoc; }
    else
    { matchLoc = maxLoc; }

    /// Show me what you got
    //rectangle( img_display, matchLoc, Point( matchLoc.x + imgTemplate.cols , matchLoc.y + imgTemplate.rows ), Scalar::all(0), 2, 8, 0 );
    //rectangle( img_result, matchLoc, Point( matchLoc.x + imgTemplate.cols , matchLoc.y + imgTemplate.rows ), Scalar::all(0), 2, 8, 0 );

    matchLocation = matchLoc;

    //imshow("img_display", img_display );
    //imshow("img_result", img_result);

    /************************************************************
    * [ROI]
    * Create a Regio of Interrest
    ************************************************************/

    /// Create ROI
    int offsetx = 50;
    int offsety = 50;
    int width = imgTemplate.cols+offsetx;
    int hight = imgTemplate.rows+offsety;
    //cout << "WH: "<< width << " " << hight << endl;

    /// Check if ROI size fits in orginal image
    if(matchLoc.x + width >= img_gray.cols|| matchLoc.y + hight >= img_gray.rows){
        //cout << "TriDetect: Can't create ROI; [width/hight > cols/rows]" << endl;
        return -1;
    }

    if(width >= img_gray.cols+matchLoc.x || hight >= img_gray.rows+matchLoc.y){
        //cout << "TriDetect: Can't create ROI; [width/hight > cols/rows]" << endl;
        return -1;
    }

    if(matchLoc.x-offsetx < 0 || matchLoc.y-offsety < 0){
        //cout << "TriDetect: Can't create ROI; [x-offset/y-offset < 0]" << endl;
        return -1;
    }
    matchLoc.x = matchLoc.x - offsetx/2;
    matchLoc.y = matchLoc.y - offsety/2;

    Rect roi = Rect(matchLoc.x,matchLoc.y,width,hight); //(x,y,width,hight)
    img_gray = img_gray(roi);
    //imshow("img_gray 2", img_gray);

    /************************************************************
    * [Corner Detection Method]
    * Detect the locations of the corners
    ************************************************************/

    /// Parameters for Shi-Tomasi algorithm
    vector<Point2f> corners;
    double qualityLevel = 0.01;
    double minDistance = 5;
    int blockSize = 3;
    bool useHarrisDetector = false;
    double k = 0.04;

    /// Apply corner detection
    int maxCorners = 3;
    goodFeaturesToTrack( img_gray,
               corners,
               maxCorners,
               qualityLevel,
               minDistance,
               Mat(),
               blockSize,
               useHarrisDetector,
               k );

    /// Draw detected corners
    //cout<< "TriDetect: Number of corners detected: " <<corners.size()<<endl;
    int r = 3;
    Mat matresult = img_gray.clone();
    threshold(matresult, matresult, 255, 255, 0);

    for( size_t i = 0; i < corners.size(); i++ )
    {
        circle( matresult, corners[i], r, Scalar(255), -1, 8, 0 );
        //cout << "TriDetect: \tX:" << Point2f(corners.at(i)).x << " Y:" << Point2f(corners.at(i)).y << endl;
        stringstream ss;
        ss << i;
        string str = ss.str();
        putText(matresult,str,Point(Point2f(corners.at(i)).x+10,Point2f(corners.at(i)).y),1,1,Scalar(255,255,255));
    }

    /// Find out of OBJ has three corners
    if(corners.size() == 3){ //triangle has 3 corners
        /// Draw line between the three points
        line(matresult,corners[0],corners[1],Scalar(255),1,8);
        line(matresult,corners[1],corners[2],Scalar(255),1,8);
        line(matresult,corners[2],corners[0],Scalar(255),1,8);

        /// Save points in triangle
        Triangle tri;
        tri.a = corners[0];
        tri.b = corners[1];
        tri.c = corners[2];

        /// Sort the points
        tri.sortLengths();

        if(tri.isValidTriangle())
        {
            /// Display lengths of A, B and C
            //cout << "AC: " << tri.AC.length << endl;
            //cout << "AB: " << tri.AB.length << endl;
            //cout << "BC: " << tri.BC.length << endl;

			angle = tri.calcAngleAC();
            stringstream sa;
            sa << "A: " << angle;
            string str2 = sa.str();

            putText(matresult,str2,Point(10,32),1,1,Scalar(255,255,255));
            //cout << "Rotation: " << str2 << endl;
            putText(matresult,"Obj: is triangle",Point(10,10),1,1,Scalar(255,255,255));
            //cout << "TriDetect: Obj is a triangle" << endl;
            writeOrientation(angle);

            /// Show what we have got
            imshow("matresult",matresult);

            TriangleMatchOke = 2;
            return 0;
        }
        else{
            imshow("matresult",matresult);
            TriangleMatchOke = 1;
            return -1;
        }
    }
    TriangleMatchOke = 0;
    return -1;
}

int detectStartStop(const Mat* src,
                    unsigned int pos[4] ){
    static unsigned int averagepos[4] = {0};

    /// clone HSV source
    Mat frameHSV = src->clone();

    /// threshold on red range
    Mat imgRed;
    inRange(frameHSV, Scalar(90,100,100), Scalar(180,255,255),imgRed); //threshold for red
    //imshow("Red DOT", imgRed);

    /// threshold on green range
    Mat imgGreen;
    inRange(frameHSV, Scalar(30,100,50), Scalar(80,255,255),imgGreen); //threshold for green
    //imshow("Green DOT", imgGreen);

    /// invert red
    threshold(imgRed, imgRed, 100, 255, 1);

    /// invert green
    threshold(imgGreen, imgGreen, 100, 255, 1);

    //imshow("Green DOT", imgGreen);
    //imshow("Red DOT", imgRed);

    /************************************************************
    * [Template Matching Method]
    * Detect the Red dot location
    ************************************************************/

    /// create the Red result matrix
    Mat imgRedResult;
    int imgRedResult_cols = imgRed.cols - imgTemplate2.cols + 1;
    int imgRedResult_rows = imgRed.rows - imgTemplate2.rows + 1;
    imgRedResult.create( imgRedResult_cols, imgRedResult_rows, CV_32FC1 );

    /// do the Red Matching and Normalize
    int match_method = 0;
    matchTemplate( imgRed, imgTemplate2, imgRedResult, match_method );
    normalize( imgRedResult, imgRedResult, 0, 1, NORM_MINMAX, -1, Mat() );

    /// localizing the best match with minMaxLoc
    double minVal; double maxVal; Point minLoc; Point maxLoc;
    minMaxLoc( imgRedResult, &minVal, &maxVal, &minLoc, &maxLoc, Mat() );

    /// for SQDIFF and SQDIFF_NORMED, the best matches are lower values. For all the other methods, the higher the better
    Point matchLocRed;
    if( match_method  == CV_TM_SQDIFF || match_method == CV_TM_SQDIFF_NORMED )
    { matchLocRed = minLoc; }
    else
    { matchLocRed = maxLoc; }

    /// Show me what you got
    //rectangle( imgRed, matchLocRed, Point( matchLocRed.x + imgTemplate2.cols , matchLocRed.y + imgTemplate2.rows ), Scalar::all(0), 2, 8, 0 );
    //rectangle( imgRedResult, matchLocRed, Point( matchLocRed.x + imgTemplate2.cols , matchLocRed.y + imgTemplate2.rows ), Scalar::all(0), 2, 8, 0 );
    //matchLocation = matchLocRed;

    /************************************************************
    * [Template Matching Method]
    * Detect the Green dot location
    ************************************************************/

    /// create the Green result matrix
    Mat imgGreenResult;
    imgGreenResult.create( imgRedResult_cols, imgRedResult_rows, CV_32FC1 );

    /// do the Green Matching and Normalize
    match_method = 0;
    matchTemplate( imgGreen, imgTemplate2, imgGreenResult, match_method );
    normalize( imgGreenResult, imgGreenResult, 0, 1, NORM_MINMAX, -1, Mat() );

    /// localizing the best match with minMaxLoc
    minMaxLoc( imgGreenResult, &minVal, &maxVal, &minLoc, &maxLoc, Mat() );

    /// for SQDIFF and SQDIFF_NORMED, the best matches are lower values. For all the other methods, the higher the better
    Point matchLocGreen;
    if( match_method  == CV_TM_SQDIFF || match_method == CV_TM_SQDIFF_NORMED )
    { matchLocGreen = minLoc; }
    else
    { matchLocGreen = maxLoc; }

    /// Show me what you got
    //rectangle( imgGreen, matchLocGreen, Point( matchLocGreen.x + imgTemplate2.cols , matchLocGreen.y + imgTemplate2.rows ), Scalar::all(0), 2, 8, 0 );
    //rectangle( imgGreenResult, matchLocGreen, Point( matchLocGreen.x + imgTemplate2.cols , matchLocGreen.y + imgTemplate2.rows ), Scalar::all(0), 2, 8, 0 );
    //matchLocation = matchLocGreen;

    //imshow("Red Result", imgRed);
    //imshow("Green Result", imgGreen);

    //Calculate average position;
    averagepos[0] = ((averagepos[0] + matchLocGreen.x)/2);
    averagepos[1] = ((averagepos[1] + matchLocGreen.y)/2);
    averagepos[2] = ((averagepos[2] + matchLocRed.x)/2);
    averagepos[3] = ((averagepos[3] + matchLocRed.y)/2);

    //Save position for match square in LiveView
    matchGreenLocation.x = averagepos[0];
    matchGreenLocation.y = averagepos[1];
    matchRedLocation.x = averagepos[2];
    matchRedLocation.y = averagepos[3];

    pos[0] = averagepos[0] + imgTemplate2.cols/2 + 12;
    pos[1] = averagepos[1] + imgTemplate2.rows/2 + 12;
    pos[2] = averagepos[2] + imgTemplate2.cols/2 + 12;
    pos[3] = averagepos[3] + imgTemplate2.rows/2 + 12;
    return 0;
}
